# -*- coding: utf-8 -*-
'''
Created on 16.01.2020

@author: yu03
'''
import numpy as np
import os
import re
import matplotlib.pyplot as plt
from scipy.optimize import curve_fit
import glob
from mpl_toolkits.mplot3d import Axes3D
from PyUeye_Unified.Cross_200line_SAAC import folder_path, np_result_names, hor_index, ver_index, hor_lines, ver_lines
import sys
from FFT_Interpolation import FFT_interpolation_nonlinearity_compare
from matplotlib import cm
from matplotlib.ticker import LinearLocator, FormatStrFormatter

# name_calibration_f = folder_path + '\\' +  'F_calibration_table' + '.dat'
# name_calibration_r = folder_path + '\\' +  'R_calibration_table' + '.dat'
# name_calibration_corrected_f = folder_path + '\\' +  'F_calibration_table_corrected' + '.dat'
# name_calibration_corrected_r = folder_path + '\\' +  'R_calibration_table_corrected' + '.dat'

def read_cal_table(name_raw):
    print('Reading Data')
    with open(name_raw,'r') as fid:
        line='%'
        while line[:6] != '%    #':
            line = fid.readline()
        out_str = fid.readlines()
    i_p, i_x, i_y, AK_K_x_Use, AK_K_x_Mod, AK_K_y_Use, AK_K_y_Mod, W_a, W_b, W_c, EN_x_m, EN_y_m = [], [], [], [], [], [], [], [], [], [], [], []
    for line in out_str:
        line_data = line.strip().split()
        a, b, c = line_data[:3]
        d, e, f, g, h, i, j = line_data[46:53]
        m, n = line_data[12], line_data[14]
        i_p.append(float(a))
        i_x.append(float(b))
        i_y.append(float(c))
        AK_K_x_Use.append(float(d))
        AK_K_x_Mod.append(float(e))
        AK_K_y_Use.append(float(f))
        AK_K_y_Mod.append(float(g))
        W_a.append(float(h))
        W_b.append(float(i))
        W_c.append(float(j))
        EN_x_m.append(float(m))
        EN_y_m.append(float(n))
    return [np.array(i_p), np.array(i_x), np.array(i_y), 
            np.array(AK_K_x_Use), np.array(AK_K_x_Mod), np.array(AK_K_y_Use), np.array(AK_K_y_Mod), 
            np.array(W_a), np.array(W_b), np.array(W_c), 
            np.array(EN_x_m), np.array(EN_y_m)]



SAAC_cali_tables = glob.glob(folder_path+'\*.dat')

sensor_x, sensor_y, reference_x, reference_y = np.empty(1), np.empty(1), np.empty(1), np.empty(1)
for f in SAAC_cali_tables:
    data_name = f.strip().split('\\')[-1].strip().split('_')
    sensor_x = np.concatenate((sensor_x, read_cal_table(f)[3]))
    sensor_y = np.concatenate((sensor_y, read_cal_table(f)[5]))
    reference_x = np.concatenate((reference_x, read_cal_table(f)[4]))
    reference_y = np.concatenate((reference_y, read_cal_table(f)[6]))
sensor_x = sensor_x[1:]
sensor_y = sensor_y[1:]
reference_x = reference_x[1:]
reference_y = reference_y[1:]

# calibration_table_f = read_cal_table(name_calibration_f)
# calibration_corrected_table_f = read_cal_table(name_calibration_corrected_f)
# calibration_table_r = read_cal_table(name_calibration_r)
# calibration_corrected_table_r = read_cal_table(name_calibration_corrected_r)
# # calibration_table_r, calibration_corrected_table_r = np.array([[],[],[],[],[],[],[],[],[],[]]),np.array([[],[],[],[],[],[],[],[],[],[]])
# 
# sensor_x = np.concatenate((calibration_table_f[3], calibration_table_r[3]))
# sensor_y = np.concatenate((calibration_table_f[5], calibration_table_r[5]))
# reference_x = np.concatenate((calibration_table_f[4], calibration_table_r[4]))
# reference_y = np.concatenate((calibration_table_f[6], calibration_table_r[6]))
# cube_x = np.concatenate((calibration_table_f[7], calibration_table_r[7]))
# cube_y = np.concatenate((calibration_table_f[8], calibration_table_r[8]))
# sensor_corrected_x = np.concatenate((calibration_corrected_table_f[3], calibration_corrected_table_r[3]))
# sensor_corrected_y = np.concatenate((calibration_corrected_table_f[5], calibration_corrected_table_r[5]))
# encoder_x = np.concatenate((calibration_corrected_table_f[9], calibration_corrected_table_r[9]))
# encoder_y = np.concatenate((calibration_corrected_table_f[10], calibration_corrected_table_r[10]))



# print(np.shape(sensor_x))
# print(np.shape(sensor_corrected_x))
# print(np.shape(reference_x))

fig = plt.figure('Raw Data')
plt.gcf().set_size_inches(18,9)

ax1 = fig.add_subplot(2, 2, 1)
ax2 = fig.add_subplot(2, 2, 2)
ax3 = fig.add_subplot(2, 2, 3)
ax4 = fig.add_subplot(2, 2, 4)

ax1.plot(sensor_x, color='blue', marker='o', markersize=0, label='sensor')
ax2.plot(sensor_y, color='blue', marker='o', markersize=2, label='sensor')
ax1.plot(reference_x, color='black', marker='o', markersize=0, label='reference')
ax2.plot(reference_y, color='black', marker='o', markersize=2, label='reference')
# ax1.plot(-cube_y, color='green', marker='o', markersize=0, label='cube')
# ax2.plot(cube_x, color='green', marker='o', markersize=2, label='cube')
# ax1.plot(sensor_corrected_x, color='red', marker='o', markersize=0, label='sensor_corrected')
# ax2.plot(sensor_corrected_y, color='red', marker='o', markersize=2, label='sensor_corrected')
# 
ax3.plot(sensor_x-reference_x, color='blue', marker='o', markersize=0, label='sensor-ref')
# ax3.plot(sensor_corrected_x-reference_x, color='red', marker='o', markersize=0, label='sensor_corrected-ref')
ax4.plot(sensor_y-reference_y, color='blue', marker='o', markersize=2, label='sensor-ref')
# ax4.plot(sensor_corrected_y-reference_y, color='red', marker='o', markersize=2, label='sensor_corrected-ref')


ax1.title.set_text('Horizontal Tilt')
ax2.title.set_text('Vertical Tilt')
ax3.title.set_text('Horizontal Tilt')
ax4.title.set_text('Vertical Tilt')
   
ax1.set_ylabel('Horizontal Tilt \ acrsec')
ax1.set_xlabel('Samples')
ax2.set_ylabel('Vertical Tilt \ acrsec')
ax2.set_xlabel('Samples')
ax3.set_ylabel('Horizontal Tilt \ acrsec')
ax3.set_xlabel('Samples')
ax4.set_ylabel('Vertical Tilt \ acrsec')
ax4.set_xlabel('Samples')

ax1.grid(which='both', axis='both')
ax2.grid(which='both', axis='both')
ax3.grid(which='both', axis='both')
ax4.grid(which='both', axis='both')

ax1.legend()
ax2.legend()
ax3.legend()
ax4.legend()

plt.tight_layout()

# plt.figure('map')
# ax = plt.axes(projection='3d')
# ax.plot_surface(encoder_x, encoder_y, sensor_corrected_x)

plt.show()



